#include "MinkowskiSum2.h"
#include "MinkowskiOperation2.h"
#include "Allocator.h"
#include <boost/ref.hpp>

//////////////////////////////////////////////////////////////////////////////

MinkowskiSum2::InternalPoint::InternalPoint()
: pointOn1_(), pointOn2_(), point_()
{
}

MinkowskiSum2::InternalPoint::InternalPoint(const MinkowskiSum2& sum)
: pointOn1_(transformedInternalPoint(*sum.p1, *sum.t1))
, pointOn2_(transformedInternalPoint(*sum.p2, *sum.t2))
, point_(pointOn1_ + pointOn2_)
{
}

MinkowskiSum2::SupportPoint::SupportPoint()
: normal_(), pointOn1_(), pointOn2_(), point_()
{
}

MinkowskiSum2::SupportPoint::SupportPoint(const MinkowskiSum2& sum, const Vector2& normal)
: normal_(normal)
, pointOn1_(transformedSupportPoint(*sum.p1, *sum.t1, normal))
, pointOn2_(transformedSupportPoint(*sum.p2, *sum.t2, normal))
, point_(pointOn1_ + pointOn2_)
{
}

const Vector2 MinkowskiSumExIm2::internalPoint() const
{
	return transformedInternalPoint(*domainA_, *tA_) + transformedInternalPoint(*domainB_, *tB_);
}

const Vector2 MinkowskiSumExIm2::supportPoint(const Vector2& normal) const
{
	return transformedSupportPoint(*domainA_, *tA_, normal) + transformedSupportPoint(*domainB_, *tB_, normal);
}
//////////////////////////////////////////////////////////////////////////////


MinkowskiSum2::MinkowskiSum2(const IConvexDomain2& p1, const Vector2& t1, const IConvexDomain2& p2, const Vector2& t2)
	: p1(&p1), t1(&t1), p2(&p2), t2(&t2)
{}

const Vector2 MinkowskiSum2::internalPoint() const
{
	InternalPoint result;
	internalPointEx(result);
	return result.point();
}

void MinkowskiSum2::internalPointEx(InternalPoint& result) const
{
	result = InternalPoint(*this);
}

const Vector2 MinkowskiSum2::supportPoint(const Vector2& normal) const
{
	SupportPoint result;
	supportPointEx(normal, result);
	return result.point();
}

void MinkowskiSum2::supportPointEx(const Vector2& normal, SupportPoint& result) const
{
	result = SupportPoint(*this, normal);
}

//////////////////////////////////////////////////////////////////////////////

MinkowskiSumExIm2::MinkowskiSumExIm2(
		const IExplicitBounded2& domainA, const Vector2& tA,
		const IImplicitBounded2& domainB, const Vector2& tB)
: domainA_(&domainA), tA_(&tA), domainB_(&domainB), tB_(&tB)
{
}

ConvolutionExIm2* MinkowskiSumExIm2::boundary(IStackAllocator& stack) const
{
	return stack.create<ConvolutionExIm2>(*domainA_, *tA_, *domainB_, *tB_, boost::ref(stack));
}
